-- FUNCTIONS --

-- Function: Pursuit & Wall Following

function PW(x, y, Speed)
	forward(y, 3*Speed/4)
	if sonarR < (3*sonarRange/4) then
		forward(x, 3*Speed/4)
		forward(y, Speed)
	end
	if sonarL > (3*sonarRange/4) then
		repeat
			forward(x, Speed)
			forward(y, 3*Speed/4)
		until sonarL > 45 or sonarR > 45
	end

end

function lightTest(sonarF, sonarR, sonarL, Speed, sonarRange)

	stop(A)
	stop(B)
	if sonarF < sonarRange then
		repeat
			backward(A, Speed)
			backward(B, Speed)
			sonarF = sonarReading(1)
		until sonarF > sonarRange
	elseif sonarR < sonarRange then
		resetRotationCount(A)
		repeat
			forward(A, 3*Speed/4) 
		until rotationCounts(A) > 365
		repeat
			forward(A, Speed)
			forward(B, Speed)
			sonarR = sonarReading(3)
		until sonarR > sonarRange
	elseif sonarL < sonarRange then
		resetRotationCount(B)
		repeat
			forward(B, 3*Speed/4)
		until rotationCounts(B) > 365
		repeat
			forward(A, Speed)
			forward(B, Speed)
			sonarL = sonarReading(2)
		until sonarL > sonarRange
	end
end

-- variables
lightMax = 60
sonarRange = 90
Speed = 50

repeat
	sonarF = sonarReading(1)
	sonarL = sonarReading(2)
	sonarR = sonarReading(3)
	light = passiveLightReading(4)

	-- LightFinding & Object Avoidance
	if light > lightMax then
		lightTest(sonarF, sonarR, sonarL, Speed, sonarRange)

	-- Directed Motion
	elseif sonarF < (3*sonarRange/4) then 
		forward(A, 3*Speed/4)
		forward(B, 3*Speed/4)
	elseif sonarF < (2*sonarRange/5) then 
		stop(A)

	-- Pursuit & Wall Following
	elseif sonarL < sonarRange then
		PW(A, B, Speed)
	elseif sonarR < sonarRange then
		PW(B, A, Speed)

	-- Wandering
	else
		a = nxt.random(2, 0)
		if a > (6/10) then
			resetRotationCount(B)
			repeat
				forward(B, Speed/2)
			until rotationCounts(B) > 50
		else
			resetRotationCount(A)
			repeat
				forward(A, Speed/2)
			until rotationCounts(A) > 50
		end
		forward(A, Speed)
		forward(B, Speed)
	end
until isPressed(ESCAPE)



